#include "Subsystems/SubsystemMovingBlocks.h"
#include "Subsystems/SubsystemTimeOfDay.h"
#include "Subsystems/SubsystemTerrain.h"
#include <cfloat>
#include "Network/PackageManager.h"
#include "Network/Packages/MovingBlockPackage.h"
#include "Log.h"

#include "GameSingleton.h"

namespace PocketSurvival
{
    SubsystemMovingBlocks::SubsystemMovingBlocks() {}

    MovingBlockSet *SubsystemMovingBlocks::addMovingBlockSet(
        const Vector3 &position,
        const Vector3 &targetPosition,
        float speed,
        float acceleration,
        float drag,
        const Vector2 &smoothness,
        std::vector<MovingBlock> &blocks,
        std::string id,
        std::shared_ptr<Point3> tag,
        bool testCollision)
    {
        MovingBlockSet *movingBlockSet = new MovingBlockSet;
        movingBlockSet->Position = position;
        movingBlockSet->StartPosition = position;
        movingBlockSet->TargetPosition = targetPosition;
        movingBlockSet->Speed = speed;
        movingBlockSet->Acceleration = acceleration;
        movingBlockSet->Drag = drag;
        movingBlockSet->Smoothness = smoothness;
        movingBlockSet->Id = id;
        movingBlockSet->Tag = tag;
        movingBlockSet->Blocks = blocks;

        movingBlockSet->updateBox();
        if(testCollision)
        {
            m_movingMutex.lock();
            movingBlocksCollision(movingBlockSet);
            m_movingMutex.unlock();

            if(movingBlockSet->Stop)
            {
                delete movingBlockSet;
                return nullptr;
            }
        }
        
        for(MovingBlock &item : blocks)
        {
            Point3 point = position.toPoint3() + item.offset;
            m_terrain->changeOldCell(point, item.value, 0);
        }

        m_movingMutex.lock();
        m_movingBlockList.push_back(movingBlockSet);
        m_movingMutex.unlock();

        GameSingleton::packageManager->addSendPackageToQueue(std::make_shared<MovingBlockPackage>(movingBlockSet));
        return movingBlockSet;
    }

    void SubsystemMovingBlocks::removeMovingBlockSet(MovingBlockSet *movingBlockSet)
    {
        m_movingMutex.lock();
        m_movingBlockList.remove(movingBlockSet);
        // for (auto it = m_movingBlockSets.begin(); it != m_movingBlockSets.end(); it++)
        // {
        //     if (*it == movingBlockSet)
        //     {
        //         m_removing.push_back(movingBlockSet);
        //         m_movingBlockSets.erase(it);
        //     }
        // }
        m_movingMutex.unlock();
        GameSingleton::packageManager->addSendPackageToQueue(std::make_shared<MovingBlockPackage>(movingBlockSet, false));
    }

    void SubsystemMovingBlocks::findMovingBlocks(const BoundingBox &boundingBox, bool extendToFillCells, std::vector<MovingBlockSet *> &result)
    {
        m_movingMutex.lock();
        for (MovingBlockSet *movingBlockSet : m_movingBlockList)
        {
            if (ExclusiveBoxIntersection(boundingBox, movingBlockSet->getBoundingBox(extendToFillCells)))
            {
                result.push_back(movingBlockSet);
            }
        }
        m_movingMutex.unlock();
    }

    MovingBlockSet *SubsystemMovingBlocks::findMovingBlocks(const std::string &id, std::shared_ptr<Point3> tag)
    {
        std::lock_guard<std::mutex> lk(m_movingMutex);
        for (MovingBlockSet *movingBlockSet : m_movingBlockList)
        {
            if (movingBlockSet->Id == id)
            {
                if (tag == nullptr)
                {
                    if (movingBlockSet->Tag == nullptr)
                    {
                        return movingBlockSet;
                    }
                }
                else if (movingBlockSet->Tag != nullptr && *tag == *(movingBlockSet->Tag))
                {
                    return movingBlockSet;
                }
            }
        }
        return nullptr;
    }

    std::shared_ptr<MovingBlocksRaycastResult> SubsystemMovingBlocks::raycast(const Vector3 &start, const Vector3 &end, bool extendToFillCells)
    {
        Ray3 ray(start, Vector3::Normalize(end - start));
        BoundingBox boundingBox(Vector3::Min(start, end), Vector3::Max(start, end));
        std::vector<MovingBlockSet *> result;
        findMovingBlocks(boundingBox, extendToFillCells, result);
        float num = FLT_MAX;
        MovingBlockSet *movingBlockSet = nullptr;
        for (MovingBlockSet *item : result)
        {
            BoundingBox box = item->getBoundingBox(extendToFillCells);
            float num2;
            if (ray.Intersection(box, num2) && num2 < num)
            {
                num = num2;
                movingBlockSet = item;
            }
        }
        if (movingBlockSet != nullptr)
        {
            std::shared_ptr<MovingBlocksRaycastResult> value = std::make_shared<MovingBlocksRaycastResult>();
            value->ray = ray;
            value->distance = num;
            value->movingBlockSet = movingBlockSet;
            return value;
        }
        return nullptr;
    }

    void SubsystemMovingBlocks::movingBlocksCollision(MovingBlockSet *movingBlockSet)
    {
        BoundingBox boundingBox = movingBlockSet->getBoundingBox(true);
        std::vector<MovingBlockSet *> result;

        for (MovingBlockSet *movingBlockSet : m_movingBlockList)
        {
            if (ExclusiveBoxIntersection(boundingBox, movingBlockSet->getBoundingBox(true)))
            {
                result.push_back(movingBlockSet);
            }
        }
        // findMovingBlocks(boundingBox, true, result);

        int32_t num = 0;
        while (true)
        {
            if (num < result.size())
            {
                if (result[num] != movingBlockSet)
                {
                    break;
                }
                num++;
                continue;
            }
            return;
        }
        movingBlockSet->stop();
    }

    void SubsystemMovingBlocks::terrainCollision(MovingBlockSet *movingBlockSet)
    {
        Point3 point, point2;
        point.posX = (int32_t)MathUtils::Floor<float>((float)movingBlockSet->MovingBox.Left + movingBlockSet->Position.posX);
        point.posY = (int32_t)MathUtils::Floor<float>((float)movingBlockSet->MovingBox.Top + movingBlockSet->Position.posY);
        point.posZ = (int32_t)MathUtils::Floor<float>((float)movingBlockSet->MovingBox.Near + movingBlockSet->Position.posZ);

        point2.posX = (int32_t)MathUtils::Ceil<float>((float)movingBlockSet->MovingBox.getRight() + movingBlockSet->Position.posX);
        point2.posY = (int32_t)MathUtils::Ceil<float>((float)movingBlockSet->MovingBox.getBottom() + movingBlockSet->Position.posY);
        point2.posZ = (int32_t)MathUtils::Ceil<float>((float)movingBlockSet->MovingBox.getFar() + movingBlockSet->Position.posZ);
        for (int32_t i = point.posX; i < point2.posX; i++)
        {
            for (int32_t j = point.posZ; j < point2.posZ; j++)
            {
                for (int32_t k = point.posY; k < point2.posY; k++)
                {
                    if (Terrain::ExtractContents(m_terrain->getCellValue(i, k, j)) != 0)
                    {
                        for (auto &func : m_collidedWithTerrainEvent)
                        {
                            func(movingBlockSet, Point3(i, k, j));
                        }
                    }
                }
            }
        }
    }

    void SubsystemMovingBlocks::SaveMovingItemToMsgPack(MovingBlockSet *movingBlockSet, nlohmann::json &jsonData)
    {
        MSG_VALUE_SET(jsonData, "Position", "Vector3", movingBlockSet->Position.toString());
        MSG_VALUE_SET(jsonData, "TargetPosition", "Vector3", movingBlockSet->TargetPosition.toString());
        MSG_VALUE_SET(jsonData, "Speed", "float", movingBlockSet->Speed);
        MSG_VALUE_SET(jsonData, "Acceleration", "float", movingBlockSet->Acceleration);
        MSG_VALUE_SET(jsonData, "Drag", "float", movingBlockSet->Drag);
        if (movingBlockSet->Smoothness != FastVector2::Zero)
        {
            MSG_VALUE_SET(jsonData, "Smoothness", "Vector2", movingBlockSet->Smoothness.toString());
        }
        if (movingBlockSet->Id != "")
        {
            MSG_VALUE_SET(jsonData, "Id", "string", movingBlockSet->Id);
        }
        if (movingBlockSet->Tag != nullptr)
        {
            MSG_VALUE_SET(jsonData, "Tag", "object", movingBlockSet->Tag->toString());
        }
        std::stringstream ss;
        for (const MovingBlock &block : movingBlockSet->Blocks)
        {
            ss << fmt::format("{},{},{},{};", block.value, block.offset.posX, block.offset.posY, block.offset.posZ);
        }
        MSG_VALUE_SET(jsonData, "Blocks", "string", ss.str());
    }


    void SubsystemMovingBlocks::update(float dt)
    {
        m_movingMutex.lock();

        for (MovingBlockSet *movingBlockSet : m_removing)
        {
            delete movingBlockSet;
        }
        m_removing.clear();

        for (MovingBlockSet *movingBlockSet : m_movingBlockList)
        {
            TerrainChunk *chunkAtCell = m_terrain->getChunkAtCell(Terrain::ToCell(movingBlockSet->Position.posX), Terrain::ToCell(movingBlockSet->Position.posZ));
            if (chunkAtCell == nullptr || chunkAtCell->State <= TerrainChunkState::InvalidContents4)
            {
                continue;
            }
            movingBlockSet->Speed += movingBlockSet->Acceleration * dt;
            if (movingBlockSet->Drag != 0.0f)
            {
                movingBlockSet->Speed *= MathUtils::Pow(1.0f - movingBlockSet->Drag, dt);
            }
            float x = Vector3::Distance(movingBlockSet->StartPosition, movingBlockSet->Position);
            float num = Vector3::Distance(movingBlockSet->TargetPosition, movingBlockSet->Position);
            float num2 = (movingBlockSet->Smoothness.posX > 0.0f) ? MathUtils::Saturate((MathUtils::Sqrt(x) + 0.05f) / movingBlockSet->Smoothness.posX) : 1.0f;
            float num3 = (movingBlockSet->Smoothness.posY > 0.0f) ? MathUtils::Saturate((num + 0.05f) / movingBlockSet->Smoothness.posY) : 1.0f;
            float num4 = num2 * num3;
            bool flag = false;
            Vector3 vector = (num > 0.0f) ? ((movingBlockSet->TargetPosition - movingBlockSet->Position) / num) : FastVector3::Zero;
            float x2 = 0.95f / dt;
            float num5 = MathUtils::Min(movingBlockSet->Speed * num4, x2);
            if (num5 * dt >= num)
            {
                movingBlockSet->Position = movingBlockSet->TargetPosition;
                movingBlockSet->CurrentVelocity = FastVector3::Zero;
                flag = true;
            }
            else
            {
                movingBlockSet->CurrentVelocity = num5 / num * (movingBlockSet->TargetPosition - movingBlockSet->Position);
                movingBlockSet->Position += movingBlockSet->CurrentVelocity * dt;
            }
            movingBlockSet->Stop = false;
            movingBlocksCollision(movingBlockSet);
            terrainCollision(movingBlockSet);
            if (movingBlockSet->Stop)
            {
                if (vector.posX < 0.0f)
                {
                    movingBlockSet->Position.posX = MathUtils::Ceil(movingBlockSet->Position.posX);
                }
                else if (vector.posX > 0.0f)
                {
                    movingBlockSet->Position.posX = MathUtils::Floor(movingBlockSet->Position.posX);
                }
                if (vector.posY < 0.0f)
                {
                    movingBlockSet->Position.posY = MathUtils::Ceil(movingBlockSet->Position.posY);
                }
                else if (vector.posY > 0.0f)
                {
                    movingBlockSet->Position.posY = MathUtils::Floor(movingBlockSet->Position.posY);
                }
                if (vector.posZ < 0.0f)
                {
                    movingBlockSet->Position.posZ = MathUtils::Ceil(movingBlockSet->Position.posZ);
                }
                else if (vector.posZ > 0.0f)
                {
                    movingBlockSet->Position.posZ = MathUtils::Floor(movingBlockSet->Position.posZ);
                }
            }
            if (movingBlockSet->Stop || flag)
            {
                m_stopped.push_back(movingBlockSet);
            }
        }
        m_movingMutex.unlock();


        for (MovingBlockSet *item : m_stopped)
        {
            for (auto &func : m_stoppedEvent)
            {
                func(item);
            }
            GameSingleton::packageManager->addSendPackageToQueue(std::make_shared<MovingBlockPackage>(item, true));
        }
        m_stopped.clear();
    }

    void SubsystemMovingBlocks::load(const nlohmann::json &json)
    {
        m_terrain = GameSingleton::singletonPtr->getSingleton<SubsystemTerrain>();
        m_timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
    }

    void SubsystemMovingBlocks::save(nlohmann::json &json) {}
    void SubsystemMovingBlocks::saveToMsgJson(nlohmann::json &jsonData) {}


    static const std::string subsystemName = "MovingBlocks";
    const std::string &SubsystemMovingBlocks::getName() const
    {
        return subsystemName;
    }
    SubUpdateType SubsystemMovingBlocks::getUpdateType()
    {
        return SubUpdateType::MultiThreadUpdate;
    }
}
